/*
 * energy_cala.cpp
 *
 *  Created on: Feb 17, 2010
 *      Author: snirgaz
 */

#include "EnergyCalc.h"

void EnergyCalc::init(QmcParameters *qmcParameters, WorldLines *wl) {
	qmcParameters_ = qmcParameters, wl_ = wl;
	resFProposed_.resize(wl_->wlData_.NumP_,
			qmcParameters->mbar.getVal());
	resFOriginal_.resize(wl_->wlData_.NumP_,
			qmcParameters->mbar.getVal());
	greenFac_ = 1.;
	forceFac_ = 1.;
}

EnergyData EnergyCalc::calcBeadsEnergy() {
#ifdef POT_FREE
	EnergyData ED;
	return ED;
#endif
	EnergyData diffEnergy;
	energyOriginal_.zero();
	energyProposed_.zero();
#ifdef EXT_POT
	this->calcExtPot(currentBeads);
#endif
	// Calc Energy for the Original Path
	energyCalcAllOriginal();
	// Calc Energy for the Proposed Path
	energyCalcAllProposed();
	energyOriginal_.calcEnergy(greenFac_, forceFac_);
	energyProposed_.calcEnergy(greenFac_, forceFac_);
	//	assert(
	//		isEnergybig(energyProposed_.energy_, energyOriginal_.energy_, QmcParams::step));
	diffEnergy = energyProposed_.diff(energyOriginal_);
	return diffEnergy;
}
void EnergyCalc::energyCalcSingleTimeSlice(ConfSlice const & wlTimeSlice,
		ParticlePos const & testParticlePos, int testParticleN, bool isOdd,
		ForceSlice & forceTimeSlice, double &totalv) {
	double f, v;
	auto itConf = wlTimeSlice.begin();
	auto itforce = forceTimeSlice.begin();
	for (; itConf != wlTimeSlice.end(); itConf++, itforce++) {
		ParticlePos diff = (*itConf) - testParticlePos;
		if (isOdd)
			CalcGreen::calcGreen(diff, v);
		else {
			CalcGreen::calcGreenForce(diff, v, f);
			*itforce = f * diff;
		}
		totalv += v;
	}
}
void EnergyCalc::energyCalcAllOriginal() {
	BeadIterVec::const_iterator itb;
	// this->countSlices(currentBeads);
	// Find Tau Bead;
	for (auto itb = moveParams_->currentBeads.begin();
			itb != moveParams_->currentBeads.end(); itb++) {
		double totalv;
		Bead &cBead = moveParams_->beads[(*itb)];
		// Get WL time Slice
		ConfSlice wlConfSlice = wl_->wlData_.confData_(cBead.WLSlice_,
				Range::all());
		// Regular Original
		ForceSlice wlforceSlice = resFOriginal_(cBead.WLSlice_, Range::all());
		energyCalcSingleTimeSlice(wlConfSlice, wlConfSlice(cBead.n_), cBead.n_,
				cBead.isOdd, wlforceSlice, totalv);
		// Jump Original
		if (cBead.originalJump_)
			energyCalcSingleTimeSlice(wlConfSlice, wl_->wlData_.jumpPos_,
					cBead.n_, cBead.isOdd, resFOriginalJump_, totalv);
		energyOriginal_.energyV_ += totalv;
	}
}
void EnergyCalc::energyCalcAllProposed() {
	BeadIterVec::const_iterator itb;
	// Find Tau Bead;
	for (auto itb = moveParams_->currentBeads.begin();
			itb != moveParams_->currentBeads.end(); itb++) {
		Bead &cBead = moveParams_->beads[(*itb)];
		double totalv;
		// Get WL time Slice
		const ConfSlice wlConfSlice = wl_->wlData_.confData_(cBead.WLSlice_,
				Range::all());
		// Regular Proposed
		ForceSlice wlforceSlice = resFProposedJump_(cBead.WLSlice_,
				Range::all());
		energyCalcSingleTimeSlice(wlConfSlice, cBead.beadPos_, cBead.n_,
				cBead.isOdd, wlforceSlice, totalv);
		// Jump Proposed
		if (cBead.proposedJump_)
			// Jump Proposed
			energyCalcSingleTimeSlice(wlConfSlice, wl_->wlData_.jumpPos_,
					cBead.n_, cBead.isOdd, resFProposedJump_, totalv);
		energyProposed_.energyV_ += totalv;
	}
}
double EnergyCalc::calcTotalFSingleSlice(ForceSlice const &allO,
		ForceSlice &allP, ForceSlice const &singleO, ForceSlice const & singleP,
		int particleNum) {
	double sumP = 0;
	for (int n = 0; n < numP_; n++) {
		if (n != particleNum)
			allP(n) = allO(n) - singleO(n) + singleP(n);
		else {
			(allP(n)).fill( 0.);
			for (int n = 0; n < numP_; n++)
				allP(n) -= singleO(n);
		}
		sumP += allP(n).sumOfSqures();
	}
	return sumP;
}
void EnergyCalc::calcTotalFallSlices() {
	Range all = Range::all();
	BeadIterVec::const_iterator itb;
	int mOriginal = 0, mProposed = 0;
	double sumO = 0, sumP = 0, currentSumP, currentSumO, jumpP, jumpO;
//	// Total Sum Of Force of Slice
//	sumO = WLForce::getTotalForceJump();
//	// Pointer to Original forces of a given Slice
//	WLForce::getJumpSliceP (allO);
// Pointer to Propo
	for (auto itb = moveParams_->currentBeads.begin();
			itb != moveParams_->currentBeads.end(); itb++) {
		Bead &cBead = moveParams_->beads[*itb];
		// Continue if not an off bead
		if (!cBead.isOdd)
			continue;
		currentSumP = 0, currentSumO = 0;
		// Total Sum Of Force of Slice
		currentSumO = wl_->wlForce_.totalForce_[cBead.WLSlice_];
		//currentSumO = WLForce::getTotalForceSlice(itb->WLSlice_);
		// Pointer to Original forces of a given Slice
		ForceSlice allO = wl_->wlForce_.forceData_(cBead.WLSlice_, all);
		//WLForce::getSliceP(itb->WLSlice_, allO);
		// Pointer to Proposed forces of a given Slice
		ForceSlice allP = moveParams_->forecUpdate.forceData_(cBead.RWSlice_,
				all);
		ForceSlice singleO = resFOriginal_(cBead.RWSlice_, all);
		ForceSlice singleP = resFProposed_(cBead.RWSlice_, all);
		// Calc Total Force
		currentSumP = calcTotalFSingleSlice(allO, allP, singleO, singleP,
				cBead.n_);
		moveParams_->forecUpdate.totalForce_[cBead.RWSlice_] = currentSumP;
#ifdef EXT_POT
		currentSumP = calcSliceForce(allO, allP, singleO, singleP,
				extPotFProposed_[itb->RWSlice_], itb->n_, QmcParams::NumP);
#elif defined(FIXED)
		currentSumP = calcSliceForce(allO, allP, singleO, singleP, itb->n_,
				QmcParams::NumPMoving, QmcParams::NumP);
#else

		//currentSumP = calcSliceForce(allO, allP, singleO, singleP, itb->n_,
		//		QmcParams::NumP);
#endif
		//for (auto it = singleP.begin(); it != singleP.end(); it++)
		//	*it += QmcParams::NumP;
		//forceUpdate_[itb->RWSlice_] = currentSumP;
		//forceUpdate_.setTotalForceSlice(itb->RWSlice_, currentSumP);
		//singleP[:]+=QmcParams::NumP;
		if (cBead.proposedJump_) {
			// WLForce::getJumpSliceP(allO);
			ForceSlice &allP = moveParams_->forecUpdate.jumpForceData_;
			//forceUpdate_.getJumpSliceP(allP);
			jumpP = calcTotalFSingleSlice(allO, allP, resFOriginalJump_,
					resFProposedJump_, cBead.n_);
			moveParams_->forecUpdate.totalForceJump_ = jumpP;
			//forceUpdate_.setTotalForceJump(jumpP);
			currentSumP = 0.5 * (jumpP + currentSumP);
		}
		if (cBead.originalJump_) {
			if (wl_->wlData_.isOpenClosed_ == OPEN) {
				jumpO = wl_->wlForce_.totalForceJump_;
				currentSumO = 0.5 * (jumpO + currentSumO);
			}
		}
		sumP += currentSumP;
		sumO += currentSumO;
	}
	energyProposed_.setEnergyF(sumP);
	energyOriginal_.setEnergyF(sumO);
}
EnergyData EnergyCalc::calcEnergySwap(int originalWL, int swapToWL) {
#ifdef POT_FREE
	EnergyData ED;
	return ED;
#endif
	EnergyData energyDiff;
//	int originalWL = MoveParams::wlStart;
//	int swapWL = MoveParams::wlSwapedStart;
	energyOriginal_.zero();
	energyProposed_.zero();
	///
	/// Original
	///

	// Get WL time Slice
	const ConfSlice wlConfSlice = wl_->wlData_.confData_(moveParams_->mStart,
			Range::all());
	// CalcFor original
	bool isOdd = moveParams_->mStart % 2;
	double totalv;
	energyCalcSingleTimeSliceSwap(wlConfSlice, wlConfSlice(originalWL),
			wl_->wlData_.jumpPos_, originalWL, swapToWL, isOdd,
			resFProposedJump_, totalv);
	energyOriginal_.energyV_ += totalv;
	// Calc For Proposed
	energyCalcSingleTimeSliceSwap(wlConfSlice, wlConfSlice(swapToWL),
			wl_->wlData_.jumpPos_, originalWL, swapToWL, isOdd,
			resFProposedJump_, totalv);
	energyProposed_.energyV_ += totalv;
#ifdef FORCE
	if (isOdd)
	this->calcTotalFSwap(originalWL, swapToWL);
#endif
	energyOriginal_.calcEnergy(qmcParameters_->greenFactorV.getVal(),
			qmcParameters_->greenFactorF.getVal());
	energyProposed_.calcEnergy(qmcParameters_->greenFactorV.getVal(),
			qmcParameters_->greenFactorF.getVal());
	// Jump Mul by .5
	energyDiff = (energyProposed_.diff(energyOriginal_)).mul(0.5);
	return energyDiff;

}

void EnergyCalc::energyCalcSingleTimeSliceSwap(const ConfSlice& wlTimeSlice,
		const ParticlePos& testParticlePos, const ParticlePos& jumpParticlePos,
		int swapToWL, int originalWL, bool isOdd, ForceSlice& forceTimeSlice,
		double& totalv) {
	double f, v;
	auto itConf = wlTimeSlice.begin();
	auto itforce = forceTimeSlice.begin();
	int n = 0;
	ParticlePos diff;
	for (; itConf != wlTimeSlice.end(); itConf++, n++) {
		// Self Interaction
		if (n == swapToWL)
			continue;
		// Interaction with jump pos
		if (n == originalWL)
			diff = jumpParticlePos - testParticlePos;
		else
			diff = (*itConf) - testParticlePos;
		if (isOdd)
			CalcGreen::calcGreen(diff, v);
		else {
			CalcGreen::calcGreenForce(diff, v, f);
			*itforce = f * diff;
			itforce++;
		}
		totalv += v;
	}
}

void EnergyCalc::calcTotalFSwap(int wlOriginal, int wlSwapTo) {
	double sumP = 0.;
	// Original forces of Swap/Jump Slice
	ForceSlice &allO = wl_->wlForce_.jumpForceData_;
	// Proposed forces of Swap/Jump Slice
	ForceSlice &allP = moveParams_->forecUpdate.jumpForceData_;
	ForceSlice &singleO = resFOriginalJump_;
	ForceSlice &singleP = resFProposedJump_;
	for (int n = 0; n < numP_; n++) {
		// Ref particle for forces
		if (n != wlSwapTo)
			allP(n) = allO(n) - singleO(n) + singleP(n);
		else {
			(allP(n)).fill(0.);
			for (int n = 0; n < numP_; n++)
				allP(n) -= singleO(n);
		}
		sumP += allP(n).sumOfSqures();
	}
	// Swap force data Check!!!!
	swap(allP(wlSwapTo), allP(wlOriginal));
	//
	energyProposed_.setEnergyF(sumP);
	energyOriginal_.setEnergyF(wl_->wlForce_.totalForceJump_);
}
#ifdef POT_AZIZ
void EnergyCalc::CalcGreen::init(double drZero) {
	drZero_ = drZero;
}
void EnergyCalc::CalcGreen::calcGreenForce(ParticlePos const & drPos, double &v,
		double &f) {
	const double D = 1.241314;
	const double A = 0.5448504E6;
	const double EPS = 10.8;
	const double ALPHA = 13.353384;
	const double C[3] = { 1.3732412, 0.4253785, 0.1781 };
	const double Cd[3] = { 1.3732412 * 6, 0.4253785 * 8, 0.1781 * 10 };
	double h, p, dp, dh, drsqr, dr_minus_ten, dr_minus_eleven, exp_arg, drS, dr;
	dr = sqrt(drPos.sumOfSqures());
	drS = dr / drZero_;
//	if (drS > drTh) {
//		v = 0;
//		f = 0;
//	} else {
	dr_minus_ten = pow(drS, -10);
	dr_minus_eleven = dr_minus_ten / drS;
	drsqr = drS * drS;
	p = dr_minus_ten * (C[2] + drsqr * (C[1] + C[0] * drsqr));
	dp = -dr_minus_eleven * (Cd[2] + drsqr * (Cd[1] + Cd[0] * drsqr));
	v = EPS * (A * exp(-ALPHA * drS));
	f = EPS * (-ALPHA * A * exp(-ALPHA * drS));
	if (drS > D) {
		v -= EPS * p;
		f -= EPS * dp;
	} else {
		exp_arg = (D / drS - 1);
		h = exp(-pow(exp_arg, 2));
		dh = 2 * D * h * exp_arg / (drsqr);
		v -= h * EPS * p;
		f -= EPS * (dh * p + h * dp);
	}
	f /= (dr * drZero_);
//}
}
void EnergyCalc::CalcGreen::calcGreen(ParticlePos const & drPos, double &v) {
	const double D = 1.241314;
	const double A = 0.5448504E6;
	const double EPS = 10.8;
	const double ALPHA = 13.353384;
	const double C[3] = { 1.3732412, 0.4253785, 0.1781 };
	double h, p, dp, dh, drsqr, dr_minus_ten, exp_arg, drS, dr;
	dr = sqrt(drPos.sumOfSqures());
	drS = dr / drZero_;
//	if (drS > drTh)
//	v = 0;
//	else {
	dr_minus_ten = pow(drS, -10);
	drsqr = drS * drS;
	p = dr_minus_ten * (C[2] + drsqr * (C[1] + C[0] * drsqr));
	v = EPS * (A * exp(-ALPHA * drS));
	if (drS > D) {
		v -= EPS * p;
	} else {
		exp_arg = (D / drS - 1);
		h = exp(-pow(exp_arg, 2));
		v -= h * EPS * p;
	}
//	}
}
#endif
